

#include "JSBSim.h"

namespace pi {

int JSBSim::runJSBSim(void)
{
    string jsbExe = path_join(m_jsbRootDir, svar.GetString("JSBSim.ExeName", "JSBSim"));
    string jsbScript = path_join("scripts", m_jsbScript);

    string cmd = fmt::sprintf("%s %s --script=%s --root=%s",
                              jsbExe, m_jsbOptions,
                              jsbScript,
                              m_jsbRootDir);
    printf("exe cmd: %s\n", cmd.c_str());
    m_jsbExe.run(cmd);
}

int JSBSim::openInputSocket(void)
{
    int ret = m_socketIn.startClient(m_addr, m_jsbIn_port, SOCKET_TCP);
    if( ret != 0 ) {
        dbg_pe("Failed to start TCP client at %s:%d", m_addr.c_str(), m_jsbIn_port);
    }

    return ret;
}

int JSBSim::openOutputSocket(void)
{
    int ret = m_socketOut.startServer(m_addr, m_jsbOut_port, SOCKET_TCP);
    if( ret != 0 ) {
        dbg_pe("Failed to start TCP server at %s:%d", m_addr.c_str(), m_jsbOut_port);
    }

    return ret;
}

int JSBSim::run(void)
{
    if( m_isRunning ) return -1;

    // open JSBSim output socket
    openOutputSocket();
    m_socketOut.setNonBlocking(1);
    tm_sleep(100);

    // run JSBSim
    runJSBSim();
    tm_sleep(1000);

    // open JSBSim input socket
    openInputSocket();

    // connect output socket
    {
        if( 0 != m_socketOut.accept(m_socketOutRecv) ) {
            dbg_pe("JSBSim output socket accept failed!");
            return -1;
        }

        RSocketAddress ca;
        m_socketOut.getClientAddress(ca);
        dbg_pt("accept a new connection! client: %s (%d)\n", ca.addr_str, ca.port);

        m_socketOutRecv.setNonBlocking(1);
    }

    // set running flag
    m_isRunning = 1;

    // start receving message thread
    start();

    // resume JSBSim
    sendMsg("resume\n");

    return 0;
}

int JSBSim::stop(void)
{
    if( !m_isRunning ) return 0;

    setAlive(0);
    wait(10);
    kill();

    m_jsbExe.stop();

    m_socketIn.close();
    m_socketOutRecv.close();
    m_socketOut.close();

    return 0;
}

int JSBSim::sendMsg(const std::string &msg)
{
    if( m_isRunning ) {
        if( msg.size() > 0 )
            m_socketIn.send((uint8_t*)msg.data(), msg.size());

        return 0;
    } else {
        return -1;
    }
}

int JSBSim::recvMsg(std::string &msg)
{
    return popMsg(msg);
}

int JSBSim::recvData(std::vector<double> &dat)
{
    std::string     msg;

    dat.clear();
    dat.reserve(64);

    int ret = popMsg(msg);
    if( ret != 0 ) return ret;

    StringArray sa = split_text(msg, ",");

    for(int i=0; i<sa.size(); i++) {
        std::string ss = trim(sa[i]);

        if( ss.size() > 0 ) dat.push_back(str_to_double(ss));
    }

    return 0;
}

int JSBSim::pushMsg(const std::string &msg)
{
    RMutex m(&m_mutexRecv);

    m_msgBuff.push_back(msg);

    return 0;
}

int JSBSim::popMsg(std::string &msg)
{
    RMutex m(&m_mutexRecv);

    if( m_msgBuff.size() > 0 ) {
        // pop old messages
        for(int i=0; i<m_msgBuff.size()-1; i++) m_msgBuff.pop_front();

        // get last message
        msg = m_msgBuff.front();
        m_msgBuff.pop_front();
        return 0;
    }

    return -1;
}

int JSBSim::thread_func(void *arg)
{
    Rate    r(svar.GetInt("JSBSim.RecvLoopFreq", 100));

    int     recvN;
    int     msgLen = svar.GetInt("JSBSim.RecvMsgBufferSize", 2048);
    char    *msgBuf;

    msgBuf = new char[msgLen];

    while( getAlive() ) {
        recvN = m_socketOutRecv.recv((uint8_t*)msgBuf, msgLen);
        if( recvN > 0 ) {
            msgBuf[recvN] = 0;
            pushMsg(msgBuf);
        }

        r.sleep();
    }

    delete [] msgBuf;

    return 0;
}

int JSBSim::init(void)
{
    m_jsbRootDir    = svar.GetString("JSBSim.RootDir", "./Data/JSBSim/");
    m_jsbOptions    = svar.GetString("JSBSim.Options", "--realtime --nice --suspend");

    m_addr          = svar.GetString("JSBSim.HostName", "127.0.0.1");
    m_jsbIn_port    = svar.GetInt("JSBSim.SocketIn.port", 5124);
    m_jsbOut_port   = svar.GetInt("JSBSim.SocketOut.port", 5123);

    m_jsbScript     = svar.GetString("JSBSim.Script", "easystar_test.xml");
    //m_jsbScript     = svar.GetString("JSBSim.Script", "c1723.xml");

    m_isRunning     = 0;

    return 0;
}

int JSBSim::release(void)
{
    m_socketIn.close();
    m_socketOut.close();
    m_socketOutRecv.close();

    if( m_isRunning ) stop();
    m_isRunning = 0;

    return 0;
}

} // end of namespace pi
